// /******************************************************************************
//  * Copyright 2017 The Apollo Authors. All Rights Reserved.
//  *
//  * Licensed under the Apache License, Version 2.0 (the "License");
//  * you may not use this file except in compliance with the License.
//  * You may obtain a copy of the License at
//  *
//  * http://www.apache.org/licenses/LICENSE-2.0
//  *
//  * Unless required by applicable law or agreed to in writing, software
//  * distributed under the License is distributed on an "AS IS" BASIS,
//  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  * See the License for the specific language governing permissions and
//  * limitations under the License.
//  *****************************************************************************/

// /**
//  * @file discretized_trajectory.cc
//  **/

// #include <trajectory/discretized_trajectory.h>

// #include <limits>

// // #include "cyber/common/log.h"
// #include "math/linear_interpolation.h"
// // #include "modules/planning/common/planning_context.h"
// #include <log.h>
// namespace opt {
// namespace planning {

// using common::TrajectoryPoint;

// DiscretizedTrajectory::DiscretizedTrajectory(
//     const std::vector<TrajectoryPoint>& trajectory_points)
//     : std::vector<TrajectoryPoint>(trajectory_points) {
//   ACHECK(!trajectory_points.empty())
//       << "trajectory_points should NOT be empty()";
// }

// // DiscretizedTrajectory::DiscretizedTrajectory(const ADCTrajectory& trajectory) {
// //   assign(trajectory.trajectory_point().begin(),
// //          trajectory.trajectory_point().end());
// // }

// TrajectoryPoint DiscretizedTrajectory::Evaluate(
//     const double relative_time) const {
//   auto comp = [](const TrajectoryPoint& p, const double relative_time) {
//     return p.relative_time() < relative_time;
//   };

//   auto it_lower = std::lower_bound(begin(), end(), relative_time, comp);

//   if (it_lower == begin()) {
//     return front();
//   } else if (it_lower == end()) {
//     AWARN << "When evaluate trajectory, relative_time(" << relative_time
//           << ") is too large";
//     return back();
//   }
//   return common::math::InterpolateUsingLinearApproximation(
//       *(it_lower - 1), *it_lower, relative_time);
// }

// size_t DiscretizedTrajectory::QueryLowerBoundPoint(const double relative_time,
//                                                    const double epsilon) const {
//   ACHECK(!empty());

//   if (relative_time >= back().relative_time()) {
//     return size() - 1;
//   }
//   auto func = [&epsilon](const TrajectoryPoint& tp,
//                          const double relative_time) {
//     return tp.relative_time() + epsilon < relative_time;
//   };
//   auto it_lower = std::lower_bound(begin(), end(), relative_time, func);
//   return std::distance(begin(), it_lower);
// }

// size_t DiscretizedTrajectory::QueryNearestPoint(
//     const common::math::Vec2d& position) const {
//   double dist_sqr_min = std::numeric_limits<double>::max();
//   size_t index_min = 0;
//   for (size_t i = 0; i < size(); ++i) {
//     const common::math::Vec2d curr_point(data()[i].path_point().x(),
//                                          data()[i].path_point().y());

//     const double dist_sqr = curr_point.DistanceSquareTo(position);
//     if (dist_sqr < dist_sqr_min) {
//       dist_sqr_min = dist_sqr;
//       index_min = i;
//     }
//   }
//   return index_min;
// }

// size_t DiscretizedTrajectory::QueryNearestPointWithBuffer(
//     const common::math::Vec2d& position, const double buffer) const {
//   double dist_sqr_min = std::numeric_limits<double>::max();
//   size_t index_min = 0;
//   for (size_t i = 0; i < size(); ++i) {
//     const common::math::Vec2d curr_point(data()[i].path_point().x(),
//                                          data()[i].path_point().y());

//     const double dist_sqr = curr_point.DistanceSquareTo(position);
//     if (dist_sqr < dist_sqr_min + buffer) {
//       dist_sqr_min = dist_sqr;
//       index_min = i;
//     }
//   }
//   return index_min;
// }

// void DiscretizedTrajectory::AppendTrajectoryPoint(
//     const TrajectoryPoint& trajectory_point) {
//   if (!empty()) {
//     CHECK_GT(trajectory_point.relative_time(), back().relative_time());
//   }
//   push_back(trajectory_point);
// }

// const TrajectoryPoint& DiscretizedTrajectory::TrajectoryPointAt(
//     const size_t index) const {
//   CHECK_LT(index, NumOfPoints());
//   return data()[index];
// }

// TrajectoryPoint DiscretizedTrajectory::StartPoint() const {
//   ACHECK(!empty());
//   return front();
// }

// double DiscretizedTrajectory::GetTemporalLength() const {
//   if (empty()) {
//     return 0.0;
//   }
//   return back().relative_time() - front().relative_time();
// }

// double DiscretizedTrajectory::GetSpatialLength() const {
//   if (empty()) {
//     return 0.0;
//   }
//   return back().path_point().s() - front().path_point().s();
// }

// }  // namespace planning
// }  // namespace opt
